# - * - coding: utf - 8 - * -
#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Point
import matplotlib.pyplot as plt
import numpy as np
from matplotlib.animation import FuncAnimation

# 初始化 ROS 节点
rospy.init_node('cross_point_sub')

# 创建 Matplotlib 图形
fig, ax = plt.subplots()
raw_line, = ax.plot([], [], label='X', color='blue')  # 设置原始数据线条为蓝色
filtered_line, = ax.plot([], [], label='Y', color='red')  # 设置滤波后数据线条为红色
ax.legend()

# 初始化数据点
x = []
raw_data = []
filtered_data = []

# 回调函数，用于处理接收到的消息
def scan_plot_callback(msg):
    global x, raw_data, filtered_data
    x.append(len(x))  # 使用列表长度作为横坐标
    raw_data.append(msg.x)
    filtered_data.append(msg.y)

# 订阅 /scan_plot 话题
rospy.Subscriber('/cross_point', Point, scan_plot_callback)

# 动画更新函数
def update(frame):
    # 更新曲线
    raw_line.set_data(x, raw_data)
    filtered_line.set_data(x, filtered_data)
    ax.relim()
    ax.autoscale_view()
    return raw_line, filtered_line

# 创建动画
ani = FuncAnimation(fig, update, interval=100)

# 显示图形
plt.show()

# 保持 ROS 节点运行
while not rospy.is_shutdown():
    rospy.spin()